#include "wheeltec_robot.h"
#include "Quaternion_Solution.h"

sensor_msgs::Imu Mpu6050;//Instantiate an IMU object //实例化IMU对象 

/**************************************
Date: January 28, 2021
Function: The main function, ROS initialization, creates the Robot_control object through the Turn_on_robot class and automatically calls the constructor initialization
功能: 主函数，ROS初始化，通过turn_on_robot类创建Robot_control对象并自动调用构造函数初始化
***************************************/
int main(int argc, char** argv)
{
  ros::init(argc, argv, "wheeltec_robot"); //ROS initializes and sets the node name //ROS初始化 并设置节点名称 
  turn_on_robot Robot_Control; //Instantiate an object //实例化一个对象
  Robot_Control.Control(); //Loop through data collection and publish the topic //循环执行数据采集和发布话题等操作
  ROS_INFO("wheeltec_robot node has turned on "); //Prompt that the wheeltec_robot node has been created //提示已创建wheeltec_robot节点 
  return 0;  
} 

/**************************************
Date: January 28, 2021
Function: Data conversion function
功能: 数据转换函数
***************************************/
short turn_on_robot::IMU_Trans(uint8_t Data_High,uint8_t Data_Low)
{
  short transition_16;
  transition_16 = 0;
  transition_16 |=  Data_High<<8;   
  transition_16 |=  Data_Low;
  return transition_16;     
}
float turn_on_robot::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
{
  float data_return;
  short transition_16;
  transition_16 = 0;
  transition_16 |=  Data_High<<8;  //Get the high 8 bits of data   //获取数据的高8位
  transition_16 |=  Data_Low;      //Get the lowest 8 bits of data //获取数据的低8位
  data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/s
  return data_return;
}
/**************************************
Date: January 28, 2021
Function: The speed topic subscription Callback function, according to the subscribed instructions through the serial port command control of the lower computer
功能: 速度话题订阅回调函数Callback，根据订阅的指令通过串口发指令控制下位机
***************************************/

//void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::TwistStamped &msg)
void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::Twist &msg)
{
	//callback every time the robot's linear velocity is received
	ros::Time current_time = ros::Time::now();

	Robot_Vel.X = msg.linear.x;
	Robot_Vel.Y = 0;
	Robot_Vel.Z = msg.angular.z;

	vel_dt = (current_time - _Last_Time).toSec();
	imu_dt = vel_dt;
	_Last_Time = current_time;

	ROS_INFO("robot: linear = %f \n",Robot_Vel.X);
	ROS_INFO("robot: angular = %f \n",Robot_Vel.Z);
	serial_pub_cmdvel.publish(msg);
}

#if 0

void turn_on_robot::Cmd_Vel_Callback(const ackermann_msgs::AckermannDriveStamped &akm_ctl)
{
  short  transition; //intermediate variable //中间变量

  //The target velocity of the X-axis of the robot
  //机器人x轴的目标线速度
  transition=0;
  transition = akm_ctl.drive.speed*1000; //Enlarge the floating point number 1000 times and transfer it //将浮点数放大一千倍再传输


  //The front wheel Angle of Ackerman car
  //阿克曼小车的前轮转角角度
  transition=0;
  transition = akm_ctl.drive.steering_angle*1000/2;

}
#endif

/**************************************
Date: 2021-11-08
获取底盘上报信息
***************************************/
void turn_on_robot::Car_Msg_Callback(const turn_on_wheeltec_robot::car &car_msg)
{
	//Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)
	Power_voltage = car_msg.Power; 
	
	//ROS_INFO("power: battery_level = %d %\n",Power_voltage);

}

/**************************************
Date: January 28, 2021
Function: Publish the IMU data topic
功能: 发布IMU数据话题
***************************************/
void turn_on_robot::Publish_ImuSensor()
{
  sensor_msgs::Imu Imu_Data_Pub; //Instantiate IMU topic data //实例化IMU话题数据
  Imu_Data_Pub.header.stamp = ros::Time::now(); 
  Imu_Data_Pub.header.frame_id = gyro_frame_id; //IMU corresponds to TF coordinates, which is required to use the robot_pose_ekf feature pack 
                                                //IMU对应TF坐标，使用robot_pose_ekf功能包需要设置此项
  Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; //A quaternion represents a three-axis attitude //四元数表达三轴姿态
  Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; 
  Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
  Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
  Imu_Data_Pub.orientation_covariance[0] = 1e6; //Three-axis attitude covariance matrix //三轴姿态协方差矩阵
  Imu_Data_Pub.orientation_covariance[4] = 1e6;
  Imu_Data_Pub.orientation_covariance[8] = 1e-6;
  Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; //Triaxial angular velocity //三轴角速度
  Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
  Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
  Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; //Triaxial angular velocity covariance matrix //三轴角速度协方差矩阵
  Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
  Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
  Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; //Triaxial acceleration //三轴线性加速度
  Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; 
  Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;  
  imu_publisher.publish(Imu_Data_Pub); //Pub IMU topic //发布IMU话题
}
/**************************************
Date: January 28, 2021
Function: Publish the odometer topic, Contains position, attitude, triaxial velocity, angular velocity about triaxial, TF parent-child coordinates, and covariance matrix
功能: 发布里程计话题，包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵
***************************************/
void turn_on_robot::Publish_Odom()
{
    //Convert the Z-axis rotation Angle into a quaternion for expression 
    //把Z轴转角转换为四元数进行表达
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z);
    nav_msgs::Odometry odom; //Instance the odometer topic data //实例化里程计话题数据

	//odom tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.frame_id = odom_frame_id;
    odom_trans.child_frame_id = robot_frame_id;
    //robot's position in x,y, and z
    odom_trans.transform.translation.x = Robot_Pos.X;
    odom_trans.transform.translation.y = Robot_Pos.Y;
    odom_trans.transform.translation.z = 0.0;
    //robot's heading in quaternion
    odom_trans.transform.rotation = odom_quat;
    odom_trans.header.stamp = ros::Time::now();_Now;
	
    //publish robot's tf using odom_trans object
    odom_broadcaster.sendTransform(odom_trans);

//odom 
    odom.header.stamp = ros::Time::now(); //_Now
    odom.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标
    odom.pose.pose.position.x = Robot_Pos.X; //Position //位置
    odom.pose.pose.position.y = Robot_Pos.Y;
    odom.pose.pose.position.z = Robot_Pos.Z;
    odom.pose.pose.orientation = odom_quat; //Posture, Quaternion converted by Z-axis rotation //姿态，通过Z轴转角转换的四元数

    odom.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标
    odom.twist.twist.linear.x = Robot_Vel.X; //Speed in the X direction //X方向速度
    odom.twist.twist.linear.y = 0.0;//Robot_Vel.Y; //Y方向速度
    odom.twist.twist.linear.z = 0.0;//Robot_Vel.Z

	odom.twist.twist.angular.x = 0.0;
	odom.twist.twist.angular.y = 0.0;
    odom.twist.twist.angular.z = Robot_Vel.Z; //绕Z轴角速度 也可以取imu 的角速度

    //There are two types of this matrix, which are used when the robot is at rest and when it is moving.Extended Kalman Filtering officially provides 2 matrices for the robot_pose_ekf feature pack
    //这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包
    if( Robot_Vel.X== 0 && Robot_Vel.Y== 0 && Robot_Vel.Z== 0 )
	{
		//If the velocity is zero, it means that the error of the encoder will be relatively small, and the data of the encoder will be considered more reliable
		//如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠
		memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),
		memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
	}
	else
	{
		//If the velocity of the trolley is non-zero, considering the sliding error that may be brought by the encoder in motion, the data of IMU is considered to be more reliable
		//如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为imu的数据更可靠
		memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),
		memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance)); 
	}

	//Pub odometer topic 发布里程计话题
    odom_publisher.publish(odom); 
}
/**************************************
Date: January 28, 2021
Function: Publish voltage-related information
功能: 发布电压相关信息
***************************************/
void turn_on_robot::Publish_Voltage()
{
	//Define the data type of the power supply voltage publishing topic 
	std_msgs::Float32 voltage_msgs; //定义电源电压发布话题的数据类型
    static float Count_Voltage_Pub=0;
	
    if(Count_Voltage_Pub++ > 10)
      {
        Count_Voltage_Pub=0;  
        voltage_msgs.data = Power_voltage; //The power supply voltage is obtained //电源供电的电压获取
        voltage_publisher.publish(voltage_msgs); //Post the power supply voltage topic unit: V, volt //发布电源电压话题单位：V、伏特
      }
}


/**************************************
Date: January 28, 2021
Function: Loop access to the lower computer data and issue topics
功能: 循环获取下位机数据与发布话题
***************************************/
void turn_on_robot::Control()
{
	_Last_Time = ros::Time::now();
	
	ros::Rate r(10);
	Robot_Pos.X = 0;
	Robot_Pos.Y = 0;
	Robot_Pos.Z = 0;
	while(ros::ok())
	{
		_Now = ros::Time::now();
		//Sampling_Time = (_Now - _Last_Time).toSec(); 
		
		//获取时间间隔，用于积分速度获得位移(里程)		
		//calculate angular displacement  θ = ω * t
		//double delta_x = (Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * vel_dt * linear_scale; //m
		//double delta_y = (Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * vel_dt * linear_scale; //m
		//double delta_theta = Robot_Vel.Z * imu_dt * angular_scale; //绕Z轴的角位移，单位：rad 

		double delta_x = (Robot_Vel.X * cos(Robot_Pos.Z)) * vel_dt * linear_scale; //m
		double delta_y = (Robot_Vel.X * sin(Robot_Pos.Z)) * vel_dt * linear_scale; //m
		double delta_theta = Robot_Vel.Z * imu_dt * angular_scale; //radians

		
		//calculate current position of the robot
		Robot_Pos.X += delta_x;
		Robot_Pos.Y += delta_y;
		Robot_Pos.Z += delta_theta;

		//通过IMU绕三轴角速度与三轴加速度计算三轴姿态
		//Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\
		//Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);

		Publish_Odom();      //Pub the speedometer topic //发布里程计话题
		//Publish_ImuSensor(); //Pub the IMU topic //发布IMU话题    
		//Publish_Voltage();   //Pub the topic of power supply voltage //发布电源电压话题

		//_Last_Time = _Now; //Record the time and use it to calculate the time interval //记录时间，用于计算时间间隔
		ros::spinOnce();   //The loop waits for the callback function //循环等待回调函数
		r.sleep();
	}
}

/**************************************
Date: January 28, 2021
Function: Constructor, executed only once, for initialization
功能: 构造函数, 只执行一次，用于初始化
***************************************/
turn_on_robot::turn_on_robot():vel_dt(0),imu_dt(0),Power_voltage(0)
{
  //Clear the data
  //清空数据
  memset(&Robot_Pos, 0, sizeof(Robot_Pos));
  memset(&Robot_Vel, 0, sizeof(Robot_Vel));
  memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));

  ros::NodeHandle private_nh("~"); //Create a node handle //创建节点句柄
  
  private_nh.param<std::string>("smoother_cmd_vel", smoother_cmd_vel, "smoother_cmd_vel"); //Smoothing control instruction, Ackerman and differential cars are used only //平滑控制指令，阿克曼、差速小车才使用到
  private_nh.param<std::string>("odom_frame_id",    odom_frame_id,    "odom");      //The odometer topic corresponds to the parent TF coordinate //里程计话题对应父TF坐标
  private_nh.param<std::string>("robot_frame_id",   robot_frame_id,   "base_link"); //The odometer topic corresponds to sub-TF coordinates //里程计话题对应子TF坐标
  private_nh.param<std::string>("gyro_frame_id",    gyro_frame_id,    "gyro_link"); //IMU topics correspond to TF coordinates //IMU话题对应TF坐标

  private_nh.param<double>("linear_scale",    linear_scale,		1.0); 
  private_nh.param<double>("angular_scale",   angular_scale,    1.0); 

  voltage_publisher = n.advertise<std_msgs::Float32>("PowerVoltage", 10); //Create a battery-voltage topic publisher //创建电池电压话题发布者
  odom_publisher = n.advertise<nav_msgs::Odometry>("odom", 50); //Create the odometer topic publisher //创建里程计话题发布者
  imu_publisher  = n.advertise<sensor_msgs::Imu>("mobile_base/sensors/imu_data", 20); //Create an IMU topic publisher //创建IMU话题发布者
  serial_pub_cmdvel  = n.advertise<geometry_msgs::Twist>("/SerialPort/cmd_vel", 100); //Create an IMU topic publisher //创建IMU话题发布者
  

  //Omnidirectional mobile trolley subscribes to the original speed McWheel /cmd_vel, without smoothing 
  //全向移动小车订阅原始速度麦轮/cmd_vel，不经过平滑处理
  Cmd_Vel_Sub = n.subscribe("cmd_vel", 100, &turn_on_robot::Cmd_Vel_Callback, this); 

  //Ackerman car subscribed to Ackerman type speed commands /ackermann_cmd(smoothed)
  //阿克曼小车订阅阿克曼类型的速度命令/ackermann_cmd(已经过平滑处理)
 // Cmd_Vel_Sub = n.subscribe("ackermann_cmd", 100, &turn_on_robot::Cmd_Vel_Callback, this); 

  //The differential car subscribed to the smoothing speed command /smoother_cmd_vel
  //差速小车订阅平滑处理后的速度命令/smoother_cmd_vel
  //Cmd_Vel_Sub = n.subscribe(smoother_cmd_vel, 100, &turn_on_robot::Cmd_Vel_Callback, this); 


  //接收前端小车上报的消息
  Car_Msg_Sub = n.subscribe("/VelSpeed", 100, &turn_on_robot::Car_Msg_Callback, this); 

  ROS_INFO_STREAM("Data ready"); //Prompt message //提示信息
  
}
/**************************************
Date: January 28, 2021
Function: Destructor, executed only once and called by the system when an object ends its life cycle
功能: 析构函数，只执行一次，当对象结束其生命周期时系统会调用这个函数
***************************************/
turn_on_robot::~turn_on_robot()
{
    ROS_INFO_STREAM("Shutting down"); //Prompt message //提示信息
	Robot_Pos.X = 0;
	Robot_Pos.Y = 0;
	Robot_Pos.Z = 0;
}

